/**
 * @author Yunfeng Bai
 *
 * 2010-11-14
 */
package geometry;

import geometry.Triangle.ClosestPointInfo;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.Set;

import javax.vecmath.Color3f;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import collision.Collision;

public class TriangleMesh {
	public static class TriangleIndex
	{
		public int _a, _b, _c;
		
		public TriangleIndex(int a_, int b_, int c_)
		{
			_a = a_;
			_b = b_;
			_c = c_;
		}
		
		public int GetIndex(int p)
		{
			if (p == 0) return _a;
			if (p == 1) return _b;
			if (p == 2) return _c;
			throw new IllegalArgumentException();
		}
	}
	
	public List<Point3d> _vertices = new ArrayList<Point3d>();
	public List<TriangleIndex> _triangleIndices = new ArrayList<TriangleIndex>();
	public List<Triangle> _triangles = new ArrayList<Triangle>();
	
	public TriangleMesh() {}
	
	public TriangleMesh(List<Triangle> triangles)
	{
		List<Point3d> vertices = new ArrayList<Point3d>();
		List<TriangleIndex> indices = new ArrayList<TriangleIndex>();
		int pos = triangles.size();
		for (Triangle t : triangles)
		{
			vertices.add(t.getA());
			vertices.add(t.getB());
			vertices.add(t.getC());
			indices.add(new TriangleIndex(pos, pos + 1, pos + 2));
			pos += 3;
		}
		init(vertices, indices);
	}
	
	public TriangleMesh(List<Point3d> vertices_, List<TriangleIndex> trianglesIndices_)
	{
		init(vertices_, trianglesIndices_);
	}
	
	public TriangleMesh(List<Point3d> vertices_, List<TriangleIndex> trianglesIndices_, Color3f color_)
	{
		init(vertices_, trianglesIndices_, color_);
	}
	
	public void init(List<Point3d> vertices_, List<TriangleIndex> trianglesIndices_)
	{
		init(vertices_, trianglesIndices_, new Color3f(0.7f, 0.7f, 1));
	}
	
	public void init(List<Point3d> vertices_, List<TriangleIndex> trianglesIndices_, Color3f color_)
	{
		_vertices = vertices_;
		_triangleIndices = trianglesIndices_;
		
		Vector3d[] verticesNormal = new Vector3d[vertices_.size()];
		@SuppressWarnings("unchecked")
		HashMap<Integer, Vector3d>[] edgesNormal = new HashMap[vertices_.size()];
		Vector3d[] trianglesNormal = new Vector3d[trianglesIndices_.size()];
		for (int i = 0; i < vertices_.size(); i++)
		{
			verticesNormal[i] = new Vector3d();
			edgesNormal[i] = new HashMap<Integer, Vector3d>();
		}
		
		Vector3d ab = new Vector3d();
		Vector3d ac = new Vector3d();
		Vector3d ba = new Vector3d();
		Vector3d bc = new Vector3d();
		Point3d a, b, c;
		double angleA, angleB, angleC;
		int ia, ib, ic;
		
		for (int i = 0; i < trianglesIndices_.size(); i++)
		{
			TriangleIndex index = trianglesIndices_.get(i);
			
			ia = index._a;
			ib = index._b;
			ic = index._c;
			
			a = _vertices.get(ia);
			b = _vertices.get(ib);
			c = _vertices.get(ic);
			
			ab.sub(b, a);
			bc.sub(c, b);
			ac.sub(c, a);
			ab.normalize();
			bc.normalize();
			ac.normalize();
			ba.negate(ab);
			
			Vector3d n = new Vector3d();
			n.cross(ab, bc);
			n.normalize();
			trianglesNormal[i] = n;
			
			angleA = Math.acos(ab.dot(ac));
			angleB = Math.acos(ba.dot(bc));
			angleC = Math.acos(ac.dot(bc));
			
			verticesNormal[ia].scaleAdd(angleA, n, verticesNormal[ia]);
			verticesNormal[ib].scaleAdd(angleB, n, verticesNormal[ib]);
			verticesNormal[ic].scaleAdd(angleC, n, verticesNormal[ic]);

			addNormalToEdge(edgesNormal, ia, ib, n);
			addNormalToEdge(edgesNormal, ib, ic, n);
			addNormalToEdge(edgesNormal, ia, ic, n);
		}
		
		for (int i = 0; i < vertices_.size(); i++)
			verticesNormal[i].normalize();
		for (int i = 0; i < vertices_.size(); i++)
			for (Vector3d n : edgesNormal[i].values())
				n.normalize();
		
		for (int i = 0; i < trianglesIndices_.size(); i++)
		{
			TriangleIndex index = trianglesIndices_.get(i);
			
			ia = index._a;
			ib = index._b;
			ic = index._c;
			
			Triangle t = new Triangle(_vertices.get(ia), _vertices.get(ib), _vertices.get(ic), color_,
					trianglesNormal[i],
					getEdgeNormal(edgesNormal, ia, ib),
					getEdgeNormal(edgesNormal, ib, ic),
					getEdgeNormal(edgesNormal, ia, ic),
					verticesNormal[ia], verticesNormal[ib], verticesNormal[ic],
					index
					);
			
			_triangles.add(t);
		}
	}
	
	private Vector3d getEdgeNormal(HashMap<Integer, Vector3d>[] edgesNormal, int a, int b)
	{
		int ma = Math.min(a, b);
		int mb = Math.max(a, b);
		return edgesNormal[ma].get(mb);
	}
	
	private void addNormalToEdge(HashMap<Integer, Vector3d>[] edgesNormal,
								int a, int b, Vector3d n)
	{
		int ma = Math.min(a, b);
		int mb = Math.max(a, b);
		
		if (edgesNormal[ma].containsKey(mb) == false)
			edgesNormal[ma].put(mb, new Vector3d(n));
		else
			edgesNormal[ma].get(mb).add(n);
	}
	
	public Collision nonPenetrationCollide(TriangleMesh mesh_)
	{
		// (TODO) get the actual contact point.
		for (Triangle triangle1 : _triangles)
		{
			for (Triangle triangle2 : mesh_._triangles)
			{
				Point3d p = triangle1.intersectWithTriangle(triangle2);
				if (p != null) return new Collision(1, null, null, p, null);
			}
		}
		return null;
	}
	
	/**
	 * Detect if there is any point in this mesh penetrate another mesh.
	 */
	public Collision penetrationCollide(TriangleMesh otherMesh)
	{
		double maxDist = Double.NEGATIVE_INFINITY;
		Triangle triangle = new Triangle();
		Point3d targetPoint = new Point3d();
		Triangle maxTriangle = null;
		Point3d maxTargetPoint = new Point3d();
		Point3d maxPenetrationPoint = new Point3d();
		for (Point3d point : _vertices)
		{
			if (otherMesh.isInside(point))
			{
				double d = otherMesh.distanceToPoint(point, triangle, targetPoint);
				if (d > maxDist)
				{
					maxDist = d;
					maxTargetPoint.set(targetPoint);
					maxTriangle = triangle;
					maxPenetrationPoint.set(point);
				}
			}
		}
		if (maxTriangle != null)
		{
			Vector3d normal = new Vector3d();
			normal.sub(maxTargetPoint, maxPenetrationPoint);
			Collision coll = new Collision(maxDist, null, null, maxPenetrationPoint, normal);
			return coll;
		}
		return null;
	}
	
	public double distanceToPoint(Point3d p, Triangle triangle, Point3d targetPoint)
	{
		double minDist = Double.POSITIVE_INFINITY;
		Point3d tmp = new Point3d();
		for (Triangle tri : _triangles)
		{
			ClosestPointInfo info = tri.getClosestPoint(p);
			double d = Math.sqrt(info.distanceSqr);
			if (d < minDist)
			{
				minDist = d;
				triangle.set(tri);
				targetPoint.set(tmp);
			}
		}
		return minDist;
	}
	
	/**
	 * Shoot a ray in a random direction,
	 * if there are odd number of intersections then the point is inside the mesh,
	 * otherwise out.
	 * 
	 * Use this method with caution!
	 * The result can be very wrong and unstable!
	 */
	public boolean isInside(Point3d p)
	{
		Vector3d d = new Vector3d(0.132132, 0.2234234, 0.5345345);
		d.normalize();
		int intersectionCount = 0;
		
		for (Triangle triangle : _triangles)
		{
			double t = triangle.intersectWithRay(p, d);
			if (t >= 0)
				intersectionCount++;
		}
		return intersectionCount % 2 == 1;
	}
	
	/**
	 * Add vertices, triangleIndices and triangles to the back of
	 * corresponding structures in this mesh.
	 */
	public void mergeWith(TriangleMesh mesh)
	{
		int vCount = _vertices.size();
		for (Point3d p : mesh._vertices)
		{
			_vertices.add(p);
		}
		for (TriangleIndex index : mesh._triangleIndices)
		{
			_triangleIndices.add(
					new TriangleIndex(index._a + vCount, index._b + vCount, index._c + vCount));
		}
		for (Triangle t : mesh._triangles)
		{
			_triangles.add(t);
		}
	}
}
